using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace XnaDevRu.Bullet
{
	public delegate bool ContactDestroyedCallback(object userPersistentData);

	public class PersistentManifold
	{
		private static ContactDestroyedCallback _contactDestroyedCallback = null;
		private static float _contactBreakingThreshold = 0.02f;

		private ManifoldPoint[] _pointCache = new ManifoldPoint[4];

		// this two body pointers can point to the physics rigidbody class.
		// object will allow any rigidbody class
		private object _bodyA;
		private object _bodyB;
		private int _cachedPoints;

		public PersistentManifold(object bodyA, object bodyB)
		{
			_bodyA = bodyA;
			_bodyB = bodyB;
			_cachedPoints = 0;
		}

		public object BodyA { get { return _bodyA; } }
		public object BodyB { get { return _bodyB; } }

		public int ContactsCount { get { return _cachedPoints; } }

		public static ContactDestroyedCallback ContactDestroyedCallback { get { return _contactDestroyedCallback; } set { _contactDestroyedCallback = value; } }
		public static float ContactBreakingThreshold { get { return _contactBreakingThreshold; } }

		public void SetBodies(object bodyA, object bodyB)
		{
			_bodyA = bodyA;
			_bodyB = bodyB;
		}

		public ManifoldPoint GetContactPoint(int index)
		{
			if (index >= _cachedPoints)
				throw new ArgumentOutOfRangeException("index", "index must be smaller than cachedPoints");

			return _pointCache[index];
		}

		public int GetCacheEntry(ManifoldPoint newPoint)
		{
			float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold;
			int size = ContactsCount;
			int nearestPoint = -1;
			for (int i = 0; i < size; i++)
			{
				ManifoldPoint mp = _pointCache[i];

				Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA;
				float distToManiPoint = Vector3.Dot(diffA, diffA);
				if (distToManiPoint < shortestDist)
				{
					shortestDist = distToManiPoint;
					nearestPoint = i;
				}
			}
			return nearestPoint;
		}

		public void AddManifoldPoint(ManifoldPoint newPoint)
		{
			if (!ValidContactDistance(newPoint))
				throw new BulletException();

			int insertIndex = ContactsCount;
			if (insertIndex == 4)
			{
				//sort cache so best points come first, based on area
				insertIndex = SortCachedPoints(newPoint);
			}
			else
			{
				_cachedPoints++;
			}
			ReplaceContactPoint(newPoint, insertIndex);
		}

		public void RemoveContactPoint(int index)
		{
			ClearUserCache(_pointCache[index]);

			int lastUsedIndex = ContactsCount - 1;
			_pointCache[index] = _pointCache[lastUsedIndex];
			//get rid of duplicated userPersistentData pointer
			_pointCache[lastUsedIndex].UserPersistentData = null;
			_cachedPoints--;
		}

		public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex)
		{
			if (!ValidContactDistance(newPoint))
				throw new BulletException();

			ClearUserCache(_pointCache[insertIndex]);

			_pointCache[insertIndex] = newPoint;
		}

		public bool ValidContactDistance(ManifoldPoint pt)
		{
			return pt.Distance <= ContactBreakingThreshold;
		}

		// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
		public void RefreshContactPoints(Matrix trA, Matrix trB)
		{
			// first refresh worldspace positions and distance
			for (int i = ContactsCount - 1; i >= 0; i--)
			{
				ManifoldPoint manifoldPoint = _pointCache[i];
				manifoldPoint.PositionWorldOnA = MathHelper.MatrixToVector(trA,manifoldPoint.LocalPointA);
				manifoldPoint.PositionWorldOnB = MathHelper.MatrixToVector(trB, manifoldPoint.LocalPointB);
				manifoldPoint.Distance = Vector3.Dot(manifoldPoint.PositionWorldOnA - manifoldPoint.PositionWorldOnB, manifoldPoint.NormalWorldOnB);
				manifoldPoint.LifeTime++;
			}

			// then 
			float distance2d;
			Vector3 projectedDifference, projectedPoint;
			for (int i = ContactsCount - 1; i >= 0; i--)
			{

				ManifoldPoint manifoldPoint = _pointCache[i];
				//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
				if (!ValidContactDistance(manifoldPoint))
				{
					RemoveContactPoint(i);
				}
				else
				{
					//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
					projectedPoint = manifoldPoint.PositionWorldOnA - manifoldPoint.NormalWorldOnB * manifoldPoint.Distance;
					projectedDifference = manifoldPoint.PositionWorldOnB - projectedPoint;
					distance2d = Vector3.Dot(projectedDifference, projectedDifference);
					if (distance2d > ContactBreakingThreshold * ContactBreakingThreshold)
					{
						RemoveContactPoint(i);
					}
				}
			}
		}

		public void ClearManifold()
		{
			for (int i = 0; i < _cachedPoints; i++)
			{
				ClearUserCache(_pointCache[i]);
			}
			_cachedPoints = 0;
		}

		private void ClearUserCache(ManifoldPoint pt)
		{
			if (pt != null)
			{
				object oldPtr = pt.UserPersistentData;

				if (oldPtr != null)
				{
					if (pt.UserPersistentData != null && _contactDestroyedCallback != null)
					{
						_contactDestroyedCallback(pt.UserPersistentData);
						pt.UserPersistentData = null;
					}
				}
			}
		}

		// sort cached points so most isolated points come first
		private int SortCachedPoints(ManifoldPoint pt)
		{
			//calculate 4 possible cases areas, and take biggest area
			//also need to keep 'deepest'

			int maxPenetrationIndex = -1;
			float maxPenetration = pt.Distance;
			for (int i = 0; i < 4; i++)
			{
				if (_pointCache[i].Distance < maxPenetration)
				{
					maxPenetrationIndex = i;
					maxPenetration = _pointCache[i].Distance;
				}
			}

			float res0 = 0, res1 = 0, res2 = 0, res3 = 0;
			if (maxPenetrationIndex != 0)
			{
				Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA;
				Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA;
				Vector3 cross = Vector3.Cross(a0, b0);
				res0 = cross.LengthSquared();
			}
			if (maxPenetrationIndex != 1)
			{
				Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA;
				Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA;
				Vector3 cross = Vector3.Cross(a1, b1);
				res1 = cross.LengthSquared();
			}

			if (maxPenetrationIndex != 2)
			{
				Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA;
				Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA;
				Vector3 cross = Vector3.Cross(a2, b2);
				res2 = cross.LengthSquared();
			}

			if (maxPenetrationIndex != 3)
			{
				Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA;
				Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA;
				Vector3 cross = Vector3.Cross(a3, b3);
				res3 = cross.LengthSquared();
			}

			Vector4 maxvec = new Vector4(res0, res1, res2, res3);
			int biggestarea = MathHelper.ClosestAxis(maxvec);
			return biggestarea;
		}

		private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return 0; }
	}
}
